/**
  ******************************************************************************
  * Copyright (C) 2020 - ~, SCUT-RobotLab Development Team
  * @file    DesiredCoMState.cpp
  * @author  Mentos Seetoo
  * @brief   			
  * @note    THe following third-party libraries are used:
  *          - Eigen3
  * @warning 
  *     - At least C++11 is required.												 
  ***************,T a)-------------------------------------------------------------*/
#include "Motion_Scheduler/DesiredCoMState.h"
/* Private defines -----------------------------------------------------------*/
#define X 0
#define Y 1
#define Z 2
#define ROLL 0
#define PITCH 1
#define YAW   2

using namespace Quadruped;

template <typename T>
static inline T limitConvert(T val, T max, T min, T deadband)
{
    if(val < deadband && val > -deadband) return 0.0;
    //else return (val/(2)) * (max - min);
    else return (val < max) ? (val < min ? min : val) : max;
}

/* Founctions ----------------------------------------------------------------*/
template <typename T>
void DesiredCoMState<T>::SetConstrains(const DesiredConstrain<T> &param)
{
    ConstrainParam = param;
}

template <typename T>
void DesiredCoMState<T>::CalCoMCommand()
{
    if(v_in(0)  -   v_d(0)  >= max_a) v_d(0) +=max_a;
        else v_d(0)=v_in(0);
    if(v_in(1)  -   v_d(1)  >= max_a) v_d(1) +=max_a;
        else v_d(1)=v_in(1);
    if(v_in(2)  -   v_d(2)  >= max_a) v_d(2) +=max_a;
        else v_d(2)=v_in(2);
    p_d += dt*v_d;
}   

template <typename T>
void DesiredCoMState<T>::SetVelocityCommand(T Vx, T Vy, T Vz,T a)
{
    max_a=a;
    v_in(X) = limitConvert(Vx, ConstrainParam.maxVx, ConstrainParam.minVx,
                                    ConstrainParam.dead_band);
    v_in(Y) = limitConvert(Vy, ConstrainParam.maxVy, ConstrainParam.minVy,
                                    ConstrainParam.dead_band);
    v_in(Z) = limitConvert(Vz, ConstrainParam.maxVz, ConstrainParam.minVz,
                                    ConstrainParam.dead_band);
}

template <typename T>
void DesiredCoMState<T>::SetAttitudeCommand(T roll, T pitch, T yaw, T h)
{
    /*
        Becareful! dAngle should never less than dead_band
        when the command values are around 0.
    */
    T dAngle = dt * ConstrainParam.maxW;

    CoM_height = limitConvert(h, ConstrainParam.maxHeight, 
                                ConstrainParam.minHeight, ConstrainParam.dead_band);
    
    if(roll - rpy_d(ROLL) > dAngle)
        rpy_d(ROLL) += dAngle;
    else
        rpy_d(ROLL) = roll;
    rpy_d(ROLL) = limitConvert(roll, ConstrainParam.maxRoll, 
                 ConstrainParam.minRoll, ConstrainParam.dead_band);

    if(pitch - rpy_d(PITCH) > dAngle)
        rpy_d(PITCH) += dAngle;
    else
        rpy_d(PITCH) = pitch;                       
    rpy_d(PITCH) = limitConvert(pitch, ConstrainParam.maxPitch,
                                ConstrainParam.minPitch, ConstrainParam.dead_band);

    if(yaw - rpy_d(YAW) > dAngle)
        rpy_d(YAW) += dAngle;
    else
        rpy_d(YAW) = yaw;                     
    rpy_d(YAW) = limitConvert(yaw, ConstrainParam.maxYaw,
                            ConstrainParam.minYaw, ConstrainParam.dead_band);
    
}

template <typename T>
void DesiredCoMState<T>::SetHeightCommand(T height)
{
    CoM_height = height;
    p_d(2) = height;
}

template <typename T>
void DesiredCoMState<T>::SetBeforeJumpHeightCommand(T beforeJumpHgt)
{
    CoM_beforeJumpHeight=beforeJumpHgt;
    p_d(2) = beforeJumpHgt;
}

template <typename T>
void DesiredCoMState<T>::SetAfterJumpHeightCommand(T afterJumpHgt)
{
    CoM_afterJumpHeight=afterJumpHgt;
    p_d(2) = afterJumpHgt;
}

template <typename T>
void DesiredCoMState<T>::SetBoundJumpHeightCommand(T boundJumpHgt)
{
    CoM_boundJumpHeight=boundJumpHgt;
    p_d(2) = boundJumpHgt;
}

template class DesiredCoMState<double>;
template class DesiredCoMState<float>;
/************************ COPYRIGHT(C) SCUT-ROBOTLAB **************************/